// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MatrixExtensionTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;

    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Unit tests for the MatrixExtension class
    /// </summary>
    [TestClass]
    public class MatrixExtensionTest
    {
        /// <summary>
        /// Test the MatrixExtension Identity
        /// </summary>
        [Priority(0)]
        [TestMethod, ExpectedException(typeof(ArgumentOutOfRangeException))]
        [TestCategory("Unit")]
        public void MatrixExtensionIdentity()
        {
            double[,] matrix = new double[3, 3];

            matrix = MatrixExtensions.GetIdentityMatrix(3);
            Assert.IsTrue(
                matrix[0, 0] == 1
                && matrix[0, 1] == 0
                && matrix[0, 2] == 0
                && matrix[1, 0] == 0
                && matrix[1, 1] == 1
                && matrix[1, 2] == 0
                && matrix[2, 0] == 0
                && matrix[2, 1] == 0
                && matrix[2, 2] == 1,
                "MatrixExtension Identity Matrix Failed");

            matrix = MatrixExtensions.GetIdentityMatrix(-1); ////this should throw
        }

        /// <summary>
        /// Test the MatrixExtension Multiply
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatrixExtensionMultiply()
        {
            double[,] matrix1 = new double[3, 3];
            double[,] matrix2 = new double[3, 3];
            double[,] matrix3 = new double[3, 4];

            Assert.IsTrue(MatrixExtensions.Multiply(matrix3, matrix2) == null, "MatrixExtension Multiply Test Failed");

            matrix1[0, 0] = 9;
            matrix1[0, 1] = 8;
            matrix1[0, 2] = 7;
            matrix1[1, 0] = 6;
            matrix1[1, 1] = 5;
            matrix1[1, 2] = 4;
            matrix1[2, 0] = 3;
            matrix1[2, 1] = 2;
            matrix1[2, 2] = 1;
            
            matrix2[0, 0] = 1;
            matrix2[0, 1] = 2;
            matrix2[0, 2] = 3;
            matrix2[1, 0] = 4;
            matrix2[1, 1] = 5;
            matrix2[1, 2] = 6;
            matrix2[2, 0] = 7;
            matrix2[2, 1] = 8;
            matrix2[2, 2] = 9;

            matrix2 = MatrixExtensions.Multiply(matrix2, matrix1);
            Assert.IsTrue(
                matrix2[0, 0] == 30
                && matrix2[0, 1] == 24
                && matrix2[0, 2] == 18
                && matrix2[1, 0] == 84
                && matrix2[1, 1] == 69
                && matrix2[1, 2] == 54
                && matrix2[2, 0] == 138
                && matrix2[2, 1] == 114
                && matrix2[2, 2] == 90,
                "MatrixExtension Multiply Failed");
        }

        /// <summary>
        /// Test the MatrixExtension Multiply by Vector
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatrixExtensionMultiplyVector()
        {
            double[,] matrix1 = new double[3, 3];
            double[] vector1 = new double[3];
            double[] vector2 = new double[4];

            Assert.IsTrue(MatrixExtensions.Multiply(matrix1, vector2) == null, "MatrixExtension Multiply by Vector Test Failed");

            matrix1[0, 0] = 9;
            matrix1[0, 1] = 8;
            matrix1[0, 2] = 7;
            matrix1[1, 0] = 6;
            matrix1[1, 1] = 5;
            matrix1[1, 2] = 4;
            matrix1[2, 0] = 3;
            matrix1[2, 1] = 2;
            matrix1[2, 2] = 1;

            vector1[0] = 1;
            vector1[1] = 2;
            vector1[2] = 3;

            vector1 = MatrixExtensions.Multiply(matrix1, vector1);
            Assert.IsTrue(
                vector1[0] == 46
                && vector1[1] == 28
                && vector1[2] == 10,
                "MatrixExtension Multiply by Vector Failed");
        }

        /// <summary>
        /// Test the MatrixExtension Subtract Test
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatrixExtensionSubtract()
        {
            double[,] matrix1 = new double[3, 3];
            double[,] matrix2 = new double[3, 3];
            double[,] matrix3 = new double[4, 4];

            double[] vector1 = new double[3];
            double[] vector2 = new double[3];
            double[] vector3 = new double[4];

            Assert.IsTrue(MatrixExtensions.Subtract(matrix1, matrix3) == null, "MatrixExtension Subtract by matrix failed");
            Assert.IsTrue(MatrixExtensions.Subtract(vector1, vector3) == null, "MatrixExtension Subtract by vector failed");

            matrix1[0, 0] = 9;
            matrix1[0, 1] = 8;
            matrix1[0, 2] = 7;
            matrix1[1, 0] = 6;
            matrix1[1, 1] = 5;
            matrix1[1, 2] = 4;
            matrix1[2, 0] = 3;
            matrix1[2, 1] = 2;
            matrix1[2, 2] = 1;

            matrix2[0, 0] = 1;
            matrix2[0, 1] = 2;
            matrix2[0, 2] = 3;
            matrix2[1, 0] = 4;
            matrix2[1, 1] = 5;
            matrix2[1, 2] = 6;
            matrix2[2, 0] = 7;
            matrix2[2, 1] = 8;
            matrix2[2, 2] = 9;

            matrix2 = MatrixExtensions.Subtract(matrix2, matrix1);

            Assert.IsTrue(
                matrix2[0, 0] == -8
                && matrix2[0, 1] == -6
                && matrix2[0, 2] == -4
                && matrix2[1, 0] == -2
                && matrix2[1, 1] == 0
                && matrix2[1, 2] == 2
                && matrix2[2, 0] == 4
                && matrix2[2, 1] == 6
                && matrix2[2, 2] == 8,
                "MatrixExtension Subtract by Matrix Failed");

            vector1[0] = 1;
            vector1[1] = 2;
            vector1[2] = 3;

            vector2[0] = 3;
            vector2[1] = 2;
            vector2[2] = 1;

            vector2 = MatrixExtensions.Subtract(vector2, vector1);

            Assert.IsTrue(
                vector2[0] == 2
                && vector2[1] == 0
                && vector2[2] == -2,
                "MatrixExtension Subtract by Vector Failed");
        }

        /// <summary>
        /// Test the MatrixExtension Add Test
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatrixExtensionAdd()
        {
            double[,] matrix1 = new double[3, 3];
            double[,] matrix2 = new double[3, 3];
            double[,] matrix3 = new double[4, 4];

            double[] vector1 = new double[3];
            double[] vector2 = new double[3];
            double[] vector3 = new double[4];

            Assert.IsTrue(MatrixExtensions.Add(matrix1, matrix3) == null, "MatrixExtension Add by matrix failed");
            Assert.IsTrue(MatrixExtensions.Add(vector1, vector3) == null, "MatrixExtension Add by vector failed");

            matrix1[0, 0] = 9;
            matrix1[0, 1] = 8;
            matrix1[0, 2] = 7;
            matrix1[1, 0] = 6;
            matrix1[1, 1] = 5;
            matrix1[1, 2] = 4;
            matrix1[2, 0] = 3;
            matrix1[2, 1] = 2;
            matrix1[2, 2] = 1;

            matrix2[0, 0] = 1;
            matrix2[0, 1] = 2;
            matrix2[0, 2] = 3;
            matrix2[1, 0] = 4;
            matrix2[1, 1] = 5;
            matrix2[1, 2] = 6;
            matrix2[2, 0] = 7;
            matrix2[2, 1] = 8;
            matrix2[2, 2] = 9;

            matrix2 = MatrixExtensions.Add(matrix2, matrix1);

            Assert.IsTrue(
                matrix2[0, 0] == 10
                && matrix2[0, 1] == 10
                && matrix2[0, 2] == 10
                && matrix2[1, 0] == 10
                && matrix2[1, 1] == 10
                && matrix2[1, 2] == 10
                && matrix2[2, 0] == 10
                && matrix2[2, 1] == 10
                && matrix2[2, 2] == 10,
                "MatrixExtension Add by Matrix Failed");

            vector1[0] = 1;
            vector1[1] = 2;
            vector1[2] = 3;

            vector2[0] = 3;
            vector2[1] = 2;
            vector2[2] = 1;

            vector2 = MatrixExtensions.Add(vector2, vector1);

            Assert.IsTrue(
                vector2[0] == 4
                && vector2[1] == 4
                && vector2[2] == 4,
                "MatrixExtension Add by Vector Failed");
        }

        /// <summary>
        /// Test the MatrixExtension Invert Test
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatrixExtensionInvert()
        {
            double[,] matrix1 = new double[3, 3];
            double[,] matrix2 = new double[4, 3];

            Assert.IsTrue(MatrixExtensions.Invert(matrix2) == null, "MatrixExtension NonSquare Matrix Invert failed");
        
            matrix1[0, 0] = 5;
            matrix1[0, 1] = 7;
            matrix1[0, 2] = 3;
            matrix1[1, 0] = 1;
            matrix1[1, 1] = 2;
            matrix1[1, 2] = 8;
            matrix1[2, 0] = 4;
            matrix1[2, 1] = 6;
            matrix1[2, 2] = 8;

            matrix1 = MatrixExtensions.Multiply(matrix1, MatrixExtensions.Invert(matrix1));
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    matrix1[i, j] = Math.Round(matrix1[i, j], 10);
                }
            }

            Assert.IsTrue(
                matrix1[0, 0] == 1
                && matrix1[0, 1] == 0
                && matrix1[0, 2] == 0
                && matrix1[1, 0] == 0
                && matrix1[1, 1] == 1
                && matrix1[1, 2] == 0
                && matrix1[2, 0] == 0
                && matrix1[2, 1] == 0
                && matrix1[2, 2] == 1, 
                "Matrix Inverse Test Failed");
        }

        /// <summary>
        /// Test the MatrixExtension Transpose Test
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatrixExtensionTranspose()
        {
            double[,] matrix1 = new double[3, 3];
            
            matrix1[0, 0] = 1;
            matrix1[0, 1] = 2;
            matrix1[0, 2] = 3;
            matrix1[1, 0] = 4;
            matrix1[1, 1] = 5;
            matrix1[1, 2] = 6;
            matrix1[2, 0] = 7;
            matrix1[2, 1] = 8;
            matrix1[2, 2] = 9;

            matrix1 = MatrixExtensions.Transpose(matrix1);

            Assert.IsTrue(
                Math.Round(matrix1[0, 0], 1) == 1
                && Math.Round(matrix1[0, 1], 1) == 4
                && Math.Round(matrix1[0, 2], 1) == 7
                && Math.Round(matrix1[1, 0], 1) == 2
                && Math.Round(matrix1[1, 1], 1) == 5
                && Math.Round(matrix1[1, 2], 1) == 8
                && Math.Round(matrix1[2, 0], 1) == 3
                && Math.Round(matrix1[2, 1], 1) == 6
                && Math.Round(matrix1[2, 2], 1) == 9,
                "MatrixExtension Transpose Failed");
        }
    }
}
